#include "ms575_algfrontss3toss8smallcircle.h"
using namespace MeasurE575;

AlgFrontSS3toSS8SmallCircle::AlgFrontSS3toSS8SmallCircle(int i):AlgBase(i)
{

}

void AlgFrontSS3toSS8SmallCircle::operator()(cv::Mat &srcImg,
                                             int id,
                                             std::map<string, float> &measRes,
                                             int fixtureId)
{
    //front  SS3-> 7,SS4-> 8, SS5-> 9,SS6-> 10 ,SS7-> 11 ,SS8-> 12
//int SmallCircle(cv::Mat srcimg, const vector<int> paras, vector<float>& res);
  //front SS345678 -> 35,36,37,38,39,40
    std::vector<int> paras7,paras8,paras9,paras10,paras11,paras12;
    for(int j=0;j< m_dataAllVec[35].m_edgeParam.cols;j++)
    {
        paras7.push_back(m_dataAllVec[35].m_edgeParam.at<int>(0,j));
        paras8.push_back(m_dataAllVec[36].m_edgeParam.at<int>(1,j));
        paras9.push_back(m_dataAllVec[37].m_edgeParam.at<int>(2,j));
        paras10.push_back(m_dataAllVec[38].m_edgeParam.at<int>(3,j));
        paras11.push_back(m_dataAllVec[39].m_edgeParam.at<int>(4,j));
        paras12.push_back(m_dataAllVec[40].m_edgeParam.at<int>(5,j));
    }
    std::vector<std::vector<int>> paras2Vec{paras7,paras8,paras9,paras10,paras11,paras12};
    std::vector<std::vector<float>> res2Vec;
    res2Vec.resize(6);
    std::vector<std::vector<cv::Rect>> rangSS2Vec;
    rangSS2Vec.resize(6);
    for(int i=0;i<6;i++)
    {
        TemplateMatch(srcImg,m_dataAllVec[i+35],rangSS2Vec[i]);
        edgedetect obj;
        obj.SmallCircle(srcImg(rangSS2Vec[i][0]),paras2Vec[i],res2Vec[i],1);
        res2Vec[i][0]+=rangSS2Vec[i][0].x;
        res2Vec[i][1]+=rangSS2Vec[i][0].y;
    }

    std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
    while (!AlgBase::flagVec[id])
    {
        (*AlgBase::cvarptrVec[id]).wait(lock);
    }

    for(auto &pt1:res2Vec)
    {
        cv::Point2f ptsIn(pt1[0],pt1[1]),ptsOut;
        CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn,ptsOut);
        pt1[0]=ptsOut.x;
        pt1[1]=ptsOut.y;
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    measRes.insert(std::make_pair(string("FAI76-SS5-X-distance_pixel"), -res2Vec[2][0]));
    float fai76ss5x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI76-SS5-X-distance")], -res2Vec[2][0]);
    measRes.insert(std::make_pair(string("FAI76-SS5-X-distance_real"), fai76ss5x));

    measRes.insert(std::make_pair(string("FAI81-SS7-Y-distance_pixel"), -res2Vec[4][1]));
    float fai81ss7y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI81-SS7-Y-distance")], -res2Vec[4][1]);
    measRes.insert(std::make_pair(string("FAI81-SS7-Y-distance_real"), fai81ss7y));

    measRes.insert(std::make_pair(string("FAI83-SS6-Y-distance_pixel"), -res2Vec[3][1]));
    float fai83ss6y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI83-SS6-Y-distance")], -res2Vec[3][1]);
    measRes.insert(std::make_pair(string("FAI83-SS6-Y-distance_real"), fai83ss6y));

    measRes.insert(std::make_pair(string("FAI84-SS8-Y-distance_pixel"), -res2Vec[5][1]));
    float fai84ss8y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI84-SS8-Y-distance")], -res2Vec[5][1]);
    measRes.insert(std::make_pair(string("FAI84-SS8-Y-distance_real"), fai84ss8y));

    measRes.insert(std::make_pair(string("FAI85-SS4-Y-distance_pixel"), -res2Vec[1][1]));
    float fai85ss4y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI85-SS4-Y-distance")], -res2Vec[1][1]);
    measRes.insert(std::make_pair(string("FAI85-SS4-Y-distance_real"), fai85ss4y));

    measRes.insert(std::make_pair(string("FAI89-SS6-X-distance_pixel"), -res2Vec[3][0]));
    float fai86ss6x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI89-SS6-X-distance")], -res2Vec[3][0]);
    measRes.insert(std::make_pair(string("FAI89-SS6-X-distance_real"), fai86ss6x));

    measRes.insert(std::make_pair(string("FAI90-SS4-X-distance_pixel"), -res2Vec[1][0]));
    float fai90ss4x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI90-SS4-X-distance")], -res2Vec[1][0]);
    measRes.insert(std::make_pair(string("FAI90-SS4-X-distance_real"), fai90ss4x));

    measRes.insert(std::make_pair(string("FAI91-SS8-X-distance_pixel"), -res2Vec[5][0]));
    float fai91ss8x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI91-SS8-X-distance")], -res2Vec[5][0]);
    measRes.insert(std::make_pair(string("FAI91-SS8-X-distance_real"), fai91ss8x));

    measRes.insert(std::make_pair(string("FAI92-SS3-X-distance_pixel"), -res2Vec[0][0]));
    float fai92ss3x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI92-SS3-X-distance")], -res2Vec[0][0]);
    measRes.insert(std::make_pair(string("FAI92-SS3-X-distance_real"), fai92ss3x));

    measRes.insert(std::make_pair(string("FAI209-SS7-X-distance_pixel"), -res2Vec[4][0]));
    float fai209ss7x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI209-SS7-X-distance")], -res2Vec[4][0]);
    measRes.insert(std::make_pair(string("FAI209-SS7-X-distance_real"), fai209ss7x));

    measRes.insert(std::make_pair(string("FAI210-SS5-Y-distance_pixel"), -res2Vec[2][1]));
    float fai210ss5y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI210-SS5-Y-distance")], -res2Vec[2][1]);
    measRes.insert(std::make_pair(string("FAI210-SS5-Y-distance_real"), fai210ss5y));

    measRes.insert(std::make_pair(string("FAI211-SS3-Y-distance_pixel"), -res2Vec[0][1]));
    float fai211ss3y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI211-SS3-Y-distance")], -res2Vec[0][1]);
    measRes.insert(std::make_pair(string("FAI211-SS3-Y-distance_real"), fai211ss3y));

    measRes.insert(std::make_pair(string("S8-diam_pixel"), res2Vec[0][2]*2));
    float s8d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S8-diam")], res2Vec[0][2]*2);
    measRes.insert(std::make_pair(string("S8-diam_real"), s8d));

    measRes.insert(std::make_pair(string("S3-diam_pixel"), res2Vec[1][2]*2));
    float s3d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S3-diam")], res2Vec[1][2]*2);
    measRes.insert(std::make_pair(string("S3-diam_real"), s3d));

    measRes.insert(std::make_pair(string("S7-diam_pixel"), res2Vec[2][2]*2));
    float s7d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S7-diam")], res2Vec[2][2]*2);
    measRes.insert(std::make_pair(string("S7-diam_real"), s7d));

    measRes.insert(std::make_pair(string("S6-diam_pixel"), res2Vec[3][2]*2));
    float s6d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S6-diam")], res2Vec[3][2]*2);
    measRes.insert(std::make_pair(string("S6-diam_real"), s6d));

    measRes.insert(std::make_pair(string("S4-diam_pixel"), res2Vec[4][2]*2));
    float s4d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S4-diam")], res2Vec[4][2]*2);
    measRes.insert(std::make_pair(string("S4-diam_real"), s4d));

    measRes.insert(std::make_pair(string("S5-diam_pixel"), res2Vec[5][2]*2));
    float s5d = CalcMeasureDistance(m_phypixpara[prefix + std::string("S5-diam")], res2Vec[5][2]*2);
    measRes.insert(std::make_pair(string("S5-diam_real"), s5d));
}
